package Strategy;

import Factory.AgriculturalMachine;
import Observer.EnvironmentData;
import java.util.*;

public class MultiMachinePathPlanner implements PathPlanningStrategy {

    private List<AgriculturalMachine> machines;
    private EnvironmentData environmentData;
    private List<PathPlanningStrategy> strategies = new ArrayList<>();

    public MultiMachinePathPlanner() {
        this.machines = new ArrayList<>();
    }

    public MultiMachinePathPlanner(List<AgriculturalMachine> machines, EnvironmentData data) {
        this.machines = machines;
        this.environmentData = data;
    }

    public void addMachine(AgriculturalMachine machine) {
        this.machines.add(machine);
    }

    public void addStrategy(PathPlanningStrategy strategy) {
        this.strategies.add(strategy);
    }

    @Override
    public Path calculatePath(FieldMap map, EnvironmentData data) {
        List<Path> paths = planPaths(map, data);
        return paths.isEmpty() ? new Path() : paths.get(0);
    }

    public List<Path> planPaths(FieldMap map, EnvironmentData data) {
        List<Path> paths = new ArrayList<>();
        if (machines.isEmpty()) {
            System.out.println("错误: 没有配置农机");
            return paths;
        }
        // 确定使用哪种策略
        PathPlanningStrategy selectedStrategy = selectStrategy(map, data);

        // 根据农机数量划分作业区域
        int machineCount = machines.size();
        double fieldWidth = map.getWidth();
        double fieldHeight = map.getHeight();
        double sectionWidth = fieldWidth / machineCount;

        System.out.println("Calculating obstacle avoidance path for " + machineCount + " machines...");

        // 为每台农机生成独立路径
        for (int i = 0; i < machineCount; i++) {
            AgriculturalMachine machine = machines.get(i);
            double startX = i * sectionWidth;
            double endX = (i + 1) * sectionWidth;

            // 生成农机的基础路径
            Path path = new Path();
            path.setName("Cooperative Path for " + machine.getType());

            // 为每台农机生成蛇形路径
            List<Coordinate> waypoints = generateSectionPath(startX, endX, fieldHeight, machine.getWorkingWidth());

            // 避开障碍物
            for (Coordinate obstacle : map.getObstaclePositions()) {
                if (isInSection(obstacle, startX, endX)) {
                  // waypoints = avoidObstacle(waypoints, obstacle, 5.0);
                }
            }

            // 设置路径点
            path.setWaypoints(waypoints);
            path.calculateTotalDistance();
            path.setEstimatedTime(path.getTotalDistance() / machine.getSpeed());

           // System.out.printf("路径总长度: %.7f米\n", path.getTotalDistance());

            paths.add(path);
        }

        // 协调路径避免冲突
        return coordinatePaths(paths);
    }

    private PathPlanningStrategy selectStrategy(FieldMap map, EnvironmentData data) {
        if (!strategies.isEmpty()) {
            // 如果有障碍物，优先使用避障策略
            for (PathPlanningStrategy strategy : strategies) {
                if (strategy instanceof ObstacleAvoidancePathPlanning) {
                    return strategy;
                }
            }
            // 否则使用第一个策略
            return strategies.get(0);
        }
        // 默认使用标准路径规划
        return new StandardPathPlanning(data);
    }

    private List<Coordinate> generateSectionPath(double startX, double endX, double fieldHeight, double machineWidth) {
        List<Coordinate> waypoints = new ArrayList<>();
        double spacing = machineWidth * 0.9; // 留出10%的重叠

        // 为每个农机生成蛇形路径
        for (double y = 0; y < fieldHeight; y += spacing) {
            if ((int)(y/spacing) % 2 == 0) {
                // 偶数行：从左到右
                waypoints.add(new Coordinate(startX, y));
                waypoints.add(new Coordinate(endX, y));
            } else {
                // 奇数行：从右到左
                waypoints.add(new Coordinate(endX, y));
                waypoints.add(new Coordinate(startX, y));
            }
        }

        return waypoints;
    }

    private boolean isInSection(Coordinate point, double startX, double endX) {
        return point.getX() >= startX && point.getX() <= endX;
    }

    private List<Path> coordinatePaths(List<Path> paths) {
        // 简化的协调算法 - 调整路径时间偏移
        List<Path> adjustedPaths = new ArrayList<>();

        for (int i = 0; i < paths.size(); i++) {
            Path originalPath = paths.get(i);
            Path adjustedPath = new Path();

            // 设置路径名称
            adjustedPath.setName(originalPath.getName() + " (协调)");

            // 根据农机索引添加时间偏移
            int offset = i * 5; // 每台农机错开5个点
            List<Coordinate> waypoints = originalPath.getWaypoints();

            for (int j = 0; j < waypoints.size(); j++) {
                adjustedPath.addWaypoint(waypoints.get((j + offset) % waypoints.size()));
            }

            adjustedPath.calculateTotalDistance();
            adjustedPath.setEstimatedTime(adjustedPath.getTotalDistance() /
                    machines.get(i).getSpeed());

            adjustedPaths.add(adjustedPath);
        }

        return adjustedPaths;
    }
}